/* Copyright 2019-2022 The MathWorks, Inc. */
#ifdef BUILDING_LIBMWCOLLISIONCODEGEN
#include "collisioncodegen/collisioncodegen_api.hpp"
#include "collisioncodegen/collisioncodegen_checkCollision_api.hpp"
#include "collisioncodegen/collisioncodegen_CollisionGeometry.hpp"
#else
#include "collisioncodegen_api.hpp"
#include "collisioncodegen_checkCollision_api.hpp"
#include "collisioncodegen_CollisionGeometry.hpp"
#endif
#include<cstring>

CollisionGeometryVoidPtr collisioncodegen_makeBox(double x, double y, double z) {
    return static_cast<void*>(new shared_robotics::CollisionGeometry(x, y, z));
}

CollisionGeometryVoidPtr collisioncodegen_makeSphere(double r) {
    return static_cast<void*>(new shared_robotics::CollisionGeometry(r));
}

CollisionGeometryVoidPtr collisioncodegen_makeCylinder(double r, double h) {
    return static_cast<void*>(new shared_robotics::CollisionGeometry(r, h));
}

CollisionGeometryVoidPtr collisioncodegen_makeCapsule(double r, double h) {
    shared_robotics::CollisionGeometry* capsule = new shared_robotics::CollisionGeometry(r, h);
    capsule->setEnumType(shared_robotics::CollisionGeometry::Type::Capsule);
    return static_cast<void*>(capsule);
}

CollisionGeometryVoidPtr collisioncodegen_makeMesh(double* vertices, double numVertices) {
    return static_cast<void*>(
        new shared_robotics::CollisionGeometry(vertices, static_cast<int>(numVertices), true));
}

void collisioncodegen_destructGeometry(const CollisionGeometryVoidPtr* objPtr) {
    // The destructor accepts a pointer to a pointer. This is required because
    // of g2415822. We are destroying what objPtr holds, and not objPtr itself.
    // Here objPtr is read-only.
    if (*objPtr != nullptr) {
        delete static_cast<shared_robotics::CollisionGeometry*>(*objPtr);
    }
}

CollisionGeometryVoidPtr collisioncodegen_copyGeometry(CollisionGeometryVoidPtr obj) {
    shared_robotics::CollisionGeometry* geom=static_cast<shared_robotics::CollisionGeometry*>(obj);
    return static_cast<void*>(new shared_robotics::CollisionGeometry(*geom));
}

int collisioncodegen_intersect(CollisionGeometryVoidPtr obj1,
                               double* pos1,
                               double* quat1,
                               CollisionGeometryVoidPtr obj2,
                               double* pos2,
                               double* quat2,
                               double computeDistance,
                               double* p1Vec,
                               double* p2Vec,
                               double* distance) {
    shared_robotics::updatePose(obj1, pos1, quat1);
    shared_robotics::updatePose(obj2, pos2, quat2);
    return shared_robotics::intersect(obj1, obj2, static_cast<int>(computeDistance), p1Vec, p2Vec,
                                      *distance);
}

void collisioncodegen_getVertices(CollisionGeometryVoidPtr obj, double* vertices){
    auto geomvertices=static_cast<const shared_robotics::CollisionGeometry*>(obj)->getVertices();
    auto numV=geomvertices.size();
    for(size_t i = 0; i < numV; ++i){
        for(size_t j = 0; j < 3; j++){
            vertices[numV*j+i]=geomvertices[i].v[j];
        }
    }
}

double collisioncodegen_getNumVertices(CollisionGeometryVoidPtr obj){
    return static_cast<double>(static_cast<const shared_robotics::CollisionGeometry*>(obj)->getNumVertices());
}

double collisioncodegen_getX(CollisionGeometryVoidPtr obj){
    return static_cast<const shared_robotics::CollisionGeometry*>(obj)->getX();
}
double collisioncodegen_getY(CollisionGeometryVoidPtr obj){
    return static_cast<const shared_robotics::CollisionGeometry*>(obj)->getY();
}
double collisioncodegen_getZ(CollisionGeometryVoidPtr obj){
    return static_cast<const shared_robotics::CollisionGeometry*>(obj)->getZ();
}
double collisioncodegen_getRadius(CollisionGeometryVoidPtr obj){
    return static_cast<const shared_robotics::CollisionGeometry*>(obj)->getRadius();
}
double collisioncodegen_getLength(CollisionGeometryVoidPtr obj){
    return static_cast<const shared_robotics::CollisionGeometry*>(obj)->getHeight();
}
double collisioncodegen_getType(CollisionGeometryVoidPtr obj, char *type){
    auto geomtype=static_cast<const shared_robotics::CollisionGeometry*>(obj)->getType();
    std::strcpy(type,geomtype.c_str());
    return static_cast<double>(geomtype.length());
}
